/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
 * This object implements a generalized UKF. The user must provide
 * the necessary input data (see constructor).
 *                      
 * FILENAME:
 * ukf.h
 *
 * PURPOSE:
 * To implement the Unscented Kalman Filter for use in state estimation.
 *
 * CREATED:
 * 10/2009 by Maciej Stachura
 *
 * LAST MODIFIED:
 * $Date: 2009/10/13 $
 * $Revision: 1.0 $
 * $Author: stachura $
*/ 

#ifndef _UKF_H
#define _UKF_H
#define MAX_UKF_DATA 100

#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <iomanip>
#include <fstream>
#include "matrix.h"

using namespace std;

// you can change this to float/double or to a template
typedef double T;

typedef struct{
  bool sig_edit;
  T alpha;
  T dataF[MAX_UKF_DATA];
  int8_t dataI[MAX_UKF_DATA];
  Matrix R, Q;
	size_t size_dataF, size_dataI;
}FiltData_t;

class UKF
{
  public:
    UKF ();
    UKF ( Matrix _x, Matrix _P, Matrix (*_f)(Matrix,FiltData_t), Matrix (*_g)(Matrix,FiltData_t), FiltData_t _param );
    ~UKF ();

    Matrix x, P;
    Matrix (*f)(Matrix, FiltData_t);
    Matrix (*g)(Matrix, FiltData_t);
    FiltData_t param;

    void time_update();
    void meas_update(Matrix z);
  private:
    T kap, bet, lam, gam, Wom, Woc, Wim, Wic, N;
    size_t n;
    Matrix X_0, X1, X2, Z1, Z2, Z_0, x_bar, P_bar, z_bar, Pzz, Pxz, K, rep;
};

#endif
